/*******************************************************************************
* Copyright 2017 CLNST CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.

*******************************************************************************/
#include <gps_driver/gps_driver.h>

#define TEST              0

#define  GD300            1
#define S1216           0

gps_RMC gps_msg;
gps_RMC gps_msg_else;//补充高度信息
unsigned int tim_H = 0;
unsigned int tim_M = 0;
unsigned int tim_S = 0;
float speed = 0;                            //米/秒

char str[20];

/**
  * @brief 
  * @param  
  * @retval 
  */
void GPSReader(const nmea_msgs::Sentence::ConstPtr& msg)
{
#if TEST
    //所有GPS信息显示
    ROS_INFO("I heard: [%s]", msg->sentence.c_str());
#else
#if GD300
    //GPRMC解析样例

    ROS_INFO("I heard: [%s]", msg->sentence.c_str());

    sscanf(msg->sentence.c_str(),"$GNGGA,,,N,,E,,,,%f,M,,M,,*",
           &(gps_msg_else.altitude)
    );
    std::cout << "*************: " << gps_msg_else.altitude << std::endl;
    sscanf(msg->sentence.c_str(),"$GNRMC,%d.00,%c,%f,N,%f,E,%f,%f,%d,%f,%c,%c*%d",
		    &(gps_msg.time),
		    &(gps_msg.state),
		    &(gps_msg.latitude),
		    &(gps_msg.longitude),
		    &(gps_msg.speed),
		    &(gps_msg.track),
		    &(gps_msg.date),
		    &(gps_msg.magnetic),
            &(gps_msg.magnetic_dir),
            &(gps_msg.mode),
            &(gps_msg.checksum)
            );
#elif S1216
    //GPRMC解析样例
    sscanf(msg->sentence.c_str(),"$GNRMC,%d.000,%c,%f,N,%f,E,%f,%f,%d,,,%c*%d",
		    &(gps_msg.time),
		    &(gps_msg.state),
		    &(gps_msg.latitude),
		    &(gps_msg.longitude),
		    &(gps_msg.speed),
		    &(gps_msg.track),
		    &(gps_msg.date),
            &(gps_msg.mode),
            &(gps_msg.checksum)
            );
            gps_msg.magnetic = 0;
            gps_msg.magnetic_dir = ' ';
#endif
#endif
    if(gps_msg.state == 'A')
    {
        tim_S = gps_msg.time%100;
        tim_M = (gps_msg.time/100)%100;
        tim_H = (gps_msg.time/10000)%100+8;
        speed = gps_msg.speed*0.5144;          //一节 = 1.852 公里/小时 = 0.5144 米/秒
        //地图经纬度参考：https://www.opengps.cn/Map/Tools/PickUpGPS_Baidu.aspx
    }
}

/**
  * @brief 
  * @param  
  * @retval 
  */
int main(int argc, char** argv)
{
    ros::init(argc, argv, "gps_driver");
    ros::NodeHandle nGPS;  
    ros::Rate loop_rate(1);

    gps_driver::Gprmc_pro gps_data;
  
    ros::Subscriber gps_sub = nGPS.subscribe("/nmea_sentence", 10, GPSReader);
    ros::Publisher gps_pub = nGPS.advertise<gps_driver::Gprmc_pro>("gps_data_pro", 100);

    while(ros::ok())
    {
      gps_data.header.stamp = ros::Time::now();
      gps_data.header.frame_id = "gps";
#if GD300
      gps_data.message_id = "GNRMC";
#elif S1216
      gps_data.message_id = "GNRMC";
#endif
      gps_data.position_status = gps_msg.state;
      //把经纬度转换到标准经纬度系下
      gps_data.lat = floor(gps_msg.latitude/100) + (gps_msg.latitude - floor(gps_msg.latitude/100)*100)/60;  //floor 向下取整
      gps_data.lon = floor(gps_msg.longitude/100) + (gps_msg.longitude - floor(gps_msg.longitude/100)*100)/60;
      gps_data.alt = gps_msg_else.altitude;  //高度信息读取
      gps_data.lat_dir = "N";
      gps_data.lon_dir = "E";
      gps_data.speed = gps_msg.speed;
      gps_data.track = gps_msg.track;
      sprintf(str, "%d", gps_msg.date);
      gps_data.date = str;
      gps_data.mag_var = gps_msg.magnetic;
      gps_data.mag_var_direction = gps_msg.magnetic_dir;
      gps_data.mode_indicator = gps_msg.mode;

      gps_pub.publish(gps_data);

// printf("本地:->[%02d:%02d:%02d] 北纬:%f  东经:%f  速度:%0.2f\r\n", 
//       tim_H, tim_M, tim_S, gps_data.lat, gps_data.lon, speed);

      ros::spinOnce();
      loop_rate.sleep();
    }

    return 0;
}
